/*
 * ARMxDSP娑堟伅闃熷垪锛孉RM绔�
 */

#include "armxdsp.h"
#include "armxdsp-impl.h"

#define ACCESS_LOCK_BYTE(lock,i) ((lock)[i])

#define MQLOCK_OTHER 1  /* 1 means DSP */
#define MQLOCK_SELF  2  /* 2 means ARM */

#ifdef KERNEL_UCLINUX
  void linux_kerenl_delay(uint32_t ms){
    uint64_t j = jiffies + ms * HZ / 1000;
    while(time_before(jiffies, j)){
      schedule();
    }
  }
#else
  void spin_idle(volatile unsigned count) {
      while (--count != 0);
  }
#endif

typedef struct armxdsp_isr_info {
    int installed;
    armxdsp_receive_handler_t handler;
    void* param;
} armxdsp_isr_info_t;

#define ISR_DSP_IMPL(name, dsp) \
    static void (name)(void);\
    static void (name)(void) { dsp_isr(dsp); }

#define INTR_REG(core_id, offset) (volatile unsigned*)(((((core_id)|8)<<28)|0x0FC02F00)+(offset))

//#define INTR_TRIGGER_WRITE(core_id, source_core_id, tpu_mask) osal_debug("INTR_TRIGGER_WRITE: %p = %x\n", INTR_REG(core_id,0x80+8*(source_core_id)), (tpu_mask));
#define INTR_TRIGGER_WRITE(core_id, source_core_id, tpu_mask)

/* 鍚戞寚瀹氱殑DSP鍐呮牳鍙戦�涓柇璇锋眰 */
static void send_interrupt_signal(int dsp) {
    INTR_TRIGGER_WRITE(dsp + 1, 0, 0xF);
}

#ifndef BPB_LIST_ADDRESS
#define BPB_LIST_ADDRESS (0x40800000-4096)
#endif

#define BPB_TAG_DSP     0x4250424C /* DSP bootloader parameters */
#define BPB_TAG_ARM     0x41425042 /* ARM OS-less bootloader parameters */
#define BPB_TAG_LINUX   0x42504203 /* Linux boot parameters */
#define BPB_TAG_DSP_MEM 0x4D616368 /* DSP memory regions info */

/*
 * 鍦―SP鎿嶄綔绯荤粺鍙互鎵惧埌鐨勪綅缃垎閰嶆秷鎭槦鍒楁寚閽堣〃鐨勫唴瀛樸� 榛樿鎯呭喌涓嬶紝璇ヨ〃瀛樺偍鍦ㄥ潡鍙傛暟锛圔PB锛夊垪琛ㄤ腑鐨勬渶鍚庝竴涓潡涔嬪悗銆� */
static armxdsp_mq_pointers_t* allocate_mq_pointers(void) {
    armxdsp_mq_pointers_t* mqp;
    uint32_t* p =  (uint32_t*)PHY_TO_ARM(BPB_LIST_ADDRESS,4096);
    /* 棣栧厛鎵惧埌BPB鍒楄〃涓殑鏈�悗涓�釜鍧�*/
    while (*p != 0) {
        switch (*p) {
            case ARMXDSP_MQ_POINTERS_TAG:
                return (armxdsp_mq_pointers_t*)p;
            case BPB_TAG_DSP_MEM:
                /* 璁＄畻DSP瀛樺偍鍖哄煙淇℃伅鍧楀ぇ灏�
聽聽聽聽聽聽聽聽聽聽聽聽聽聽聽聽聽聽聽璇ュ潡鐢�涓瓧鐨勫ご缁勬垚锛屾瘡涓尯鍩熷姞涓�涓瓧 */
                p = (uint32_t*)((char*)p
                    + 7*sizeof(uint32_t) /* 澶�*/
                    + p[5]*3*sizeof(uint32_t)); /* 鍖哄煙 */
                break;
            case BPB_TAG_DSP:
            case BPB_TAG_ARM:
            case BPB_TAG_LINUX:
                /* 杩欏潡澶у皬锛堜互瀛楄妭涓哄崟浣嶏級瀛樺偍鍦ㄧ浜屼釜瀛椾腑 */
                if (p[1] != 0) {
                    p = (uint32_t*)((char*)p + p[1]);
                    continue;
                }
            default:
                return NULL;
        }
    }
    /* 鍦˙PB鍒楄〃涓殑鏈�悗涓�釜鍧椾箣鍚庯紝p鐐圭珛鍗宠鍒嗛厤 */
    mqp = (armxdsp_mq_pointers_t*)p;
    /* 鍒濆鍖栨湯灏剧殑琛ㄥご */
    mqp->tag = ARMXDSP_MQ_POINTERS_TAG;
    mqp->size = sizeof(armxdsp_mq_pointers_t);
    *(uint32_t*)(mqp + 1) = 0;
    return mqp;
}

static int armxdsp_export_queues(void) {
    int i;
    armxdsp_mq_pointers_t* p = allocate_mq_pointers();
    if (p == NULL) {
        return ARMXDSP_EINIT;
    }
    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        p->dsp_to_arm[i] = (armxdsp_mq_t *)VIR_TO_PHY(in_queues[i]);
        // printk("cdev_armxdsp: dsp_to_arm[%d] =%32x\n", i, VIR_TO_PHY(in_queues[i]));
    }
    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        p->arm_to_dsp[i] = (armxdsp_mq_t *)VIR_TO_PHY(out_queues[i]);
        //  printk("cdev_armxdsp: arm_to_dsp[%d] =%32x\n", i, VIR_TO_PHY(out_queues[i]));
    }
    return 0;
}

static void mqlock_init(volatile uint8_t* lock) {
    lock[0] = lock[1] = lock[2] = lock[3] = 0;
}

#ifndef ACCESS_LOCK_BYTE
#define ACCESS_LOCK_BYTE(lock,i) ((lock)[i])
#endif

static void mqlock_lock(volatile uint8_t* lock) {
    ACCESS_LOCK_BYTE(lock, MQLOCK_SELF) = 1;
    ACCESS_LOCK_BYTE(lock, 0) = MQLOCK_OTHER;
    while (ACCESS_LOCK_BYTE(lock, MQLOCK_OTHER) != 0  && ACCESS_LOCK_BYTE(lock, 0) == MQLOCK_OTHER) {
        POLL_DELAY();
    }
}

static void mqlock_unlock(volatile uint8_t* lock) {
    ACCESS_LOCK_BYTE(lock, MQLOCK_SELF) = 0;
}

static size_t armxdsp_get_config_size(const armxdsp_mq_config_t* config) {
    uint32_t i;
    size_t size;
    if (config == NULL || config->slot_count == 0 || config->slot_count > ARMXDSP_MAX_SLOT_COUNT) {
        return 0;
    }
    size = ARMXDSP_MQ_SIZE(config->slot_count);
    for (i = 0; i < config->slot_count; ++i) {
        if (config->slot_size[i] < ARMXDSP_MIN_SLOT_SIZE) {
            return 0;
        }
        size += ARMXDSP_SLOT_SIZE(config->slot_size[i]);
    }
    return size;
}

static int armxdsp_use_outq(int dsp, void* mq_addr) {
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
    out_queues[dsp] = (armxdsp_mq_t*)mq_addr;
    return 0;
}

static int armxdsp_use_inq(int dsp, void* mq_addr) {
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
    in_queues[dsp] = (armxdsp_mq_t*)mq_addr;
    return 0;
}

static int mq_init(void* mq_addr, const armxdsp_mq_config_t* config) {
    uint32_t i;
    uint32_t a;
    armxdsp_mq_t* q;
    if (config == NULL) {
        return ARMXDSP_EINVAL;
    }
    if (config->slot_count == 0 || config->slot_count > ARMXDSP_MAX_SLOT_COUNT) {
        return ARMXDSP_ECONFIG;
    }
    for (i = 0; i < config->slot_count; ++i) {
        if (config->slot_size[i] < ARMXDSP_MIN_SLOT_SIZE) {
            return ARMXDSP_ECONFIG;
        }
    }
    q = (armxdsp_mq_t*)mq_addr;
    q->slot_count = config->slot_count;
    q->msg_ready = 0;
    mqlock_init(q->lock);
    a = ARMXDSP_MQ_SIZE(config->slot_count);
    for (i = 0; i < config->slot_count; ++i) {
        armxdsp_mq_slot_t* s = &q->slots[i];
        s->size = config->slot_size[i];
        s->address = a;
        s->used_size = 0;
        s->wpos = 0;
        s->rpos = 0;
        s->padding[0] = 0;
        s->padding[1] = 0;
        s->padding[2] = 0;
        a += ARMXDSP_SLOT_SIZE(config->slot_size[i]);
    }
    return 0;
}

static int armxdsp_init_outq(int dsp, void* mq_addr, const armxdsp_mq_config_t* config) {
    int result;
    result = mq_init(mq_addr, config);
    if (result != 0) {
        return result;
    }
    return armxdsp_use_outq(dsp, mq_addr);
}

static int armxdsp_init_inq(int dsp, void* mq_addr, const armxdsp_mq_config_t* config) {
    int result;
    result = mq_init(mq_addr, config);
    if (result != 0) {
        return result;
    }
    return armxdsp_use_inq(dsp, mq_addr);
}

int armxdsp_send_begin(armxdsp_sender_t* sender, int dsp, int slot,
        uint32_t size) {
    for (;;) {
        int status = armxdsp_send_begin_nb(sender, dsp, slot, size);
        if (status != ARMXDSP_EAGAIN) {
            return status;
        }
        BLOCK_DELAY();
    }
}

int armxdsp_send_begin_nb(armxdsp_sender_t* sender, int dsp, int slot_,
        uint32_t size) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t used_size;
    uint32_t above_wpos;
    uint32_t* data;
    uint32_t slot = slot_;

#if ARMXDSP_CHECK_ARGS != 0
    if (sender == NULL) {
        return ARMXDSP_EINVAL;
    }
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
#endif
    q = out_queues[dsp];
#if ARMXDSP_CHECK_ARGS != 0
    if (q == NULL) {
        return ARMXDSP_EDSP;
    }
    if (slot < 0 || slot >= q->slot_count) {
        return ARMXDSP_ESLOT;
    }
#endif
    s = &q->slots[slot];
    used_size = MESSAGE_SIZE(size);
#if ARMXDSP_CHECK_ARGS != 0
    if (used_size > s->size) {
        return ARMXDSP_ESIZE;
    }
#endif
    above_wpos = s->size - s->wpos;
    if (used_size > above_wpos) {
        if (used_size + above_wpos > GET_SLOT_FREE_SPACE(s)) {
            if (s->used_size != 0) {
                return ARMXDSP_EAGAIN;
            }
            mqlock_lock(q->lock);
            s->wpos = 0;
            s->rpos = 0;
            mqlock_unlock(q->lock);
        } else {
            *GET_SLOT_DATA(q,s,s->wpos) = (uint32_t)(-(int32_t)above_wpos);
            used_size += above_wpos;
        }
        data = GET_SLOT_DATA(q,s,0);
    } else {
        if (used_size > GET_SLOT_FREE_SPACE(s)) {
            return ARMXDSP_EAGAIN;
        }
        data = GET_SLOT_DATA(q,s,s->wpos);
    }
    *data++ = size;
    sender->dsp = dsp;
    sender->slot = slot;
    sender->size = size;
    sender->data = data;
    sender->used_size = used_size;
    return 0;
}

void armxdsp_send_end(armxdsp_sender_t* sender) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t ready;
    uint32_t wpos;
    q = out_queues[sender->dsp];
    s = &q->slots[sender->slot];
    mqlock_lock(q->lock);
    s->used_size += sender->used_size;
    wpos = s->wpos + sender->used_size;
    if (wpos >= s->size) {
        /* wrap around wpos */
        wpos -= s->size;
    }
    s->wpos = wpos;
    ready = q->msg_ready;
    q->msg_ready = ready | (1 << sender->slot);
    if (ready == 0) {
        send_interrupt_signal(sender->dsp);
    }
    mqlock_unlock(q->lock);
}

int armxdsp_receive_begin(armxdsp_receiver_t* receiver, int dsp, uint32_t slotmask) {
    for (;;) {
        int status = armxdsp_receive_begin_nb(receiver, dsp, slotmask);
        if (status != ARMXDSP_EAGAIN) {
            return status;
        }
         BLOCK_DELAY();
    }
}

int armxdsp_receive_begin_nb(armxdsp_receiver_t* receiver, int dsp, uint32_t slotmask) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t* data;
    uint32_t ready;
    uint32_t size;
    uint32_t used_size;
    int ready_slot;
#if ARMXDSP_CHECK_ARGS != 0
    if (receiver == NULL) {
        return ARMXDSP_EINVAL;
    }
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
#endif
    q = in_queues[dsp];
#if ARMXDSP_CHECK_ARGS != 0
    if (q == NULL) {
        return ARMXDSP_EDSP;
    }
#endif
    ready = q->msg_ready & slotmask;
    if (ready == 0) {
        return ARMXDSP_EAGAIN;
    }
    ready_slot = 0;
    while ((ready & 1) == 0 && ready_slot < (int)q->slot_count) {
        ++ready_slot;
        ready >>= 1;
    }
    s = &q->slots[ready_slot];
    used_size = 0;
    data = GET_SLOT_DATA(q,s,s->rpos);
    if ((int32_t)*data < 0) {
        uint32_t rpos = s->rpos - *data;
        used_size -= *data;
        if (rpos >= s->size) {
            rpos -= s->size;
        }
        data = GET_SLOT_DATA(q,s,rpos);
    }
    size = *data++;
    used_size += MESSAGE_SIZE(size);
    receiver->dsp = dsp;
    receiver->slot = ready_slot;
    receiver->size = size;
    receiver->data = data;
    receiver->used_size = used_size;
    return 0;
}

void armxdsp_receive_end(armxdsp_receiver_t* receiver) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t rpos;
    q = in_queues[receiver->dsp];
    s = &q->slots[receiver->slot];
    mqlock_lock(q->lock);
    s->used_size -= receiver->used_size;
    rpos = s->rpos + receiver->used_size;
    if (rpos >= s->size) {
        /* wrap around rpos */
        rpos -= s->size;
        // if (rpos >= s->size) {
        //  printf("OST LTE Stack: %s:(%d) Assert failed: %s rpos=%d s->size=%d",
        //          __FILE__, __LINE__, "rpos < s->size",
        //          rpos, s->size
        //         );
        //   while( 1 );
        //   }
        // ARMXDSP_ASSERT(rpos < s->size);
    }
    s->rpos = rpos;
    if (s->used_size == 0) {
        q->msg_ready &= ~(1 << receiver->slot);
    }
    mqlock_unlock(q->lock);
}

/* 杩欐槸ARMxDSP闃熷垪绠＄悊鐨凙RM绔�*/

/**
 * @brief 鍒涘缓ARMxDSP闃熷垪銆� *
 * 涓篈RMxDSP闃熷垪鍒涘缓鍜屽垎閰嶅唴瀛樸�
 *
 * @return 0锛氭棤閿欒.
 */
int ost_initDSP_mq(void)
{
    int i;
    size_t mqsize;
    void* mqmem;
    armxdsp_mq_config_t mqcfg = {
        4, { MSGQ_BUF_SIZE*8, MSGQ_BUF_SIZE*8,
             MSGQ_BUF_SIZE*8, MSGQ_BUF_SIZE*8 }
    };
    mqsize = armxdsp_get_config_size(&mqcfg);
    if (mqsize == 0) {
        return -1;
    }
    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        mqmem = mq_alloc(mqsize);
        if (mqmem == NULL) {
            return -2;
        }
        if (armxdsp_init_inq(i, mqmem, &mqcfg)) {
            return -3;
        }
    }
    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        mqmem = mq_alloc(mqsize);
        if (mqmem == NULL) {
            return -2;
        }
        if (armxdsp_init_outq(i, mqmem, &mqcfg)) {
            return -3;
        }
    }
    return 0;
}

/**
 * @brief 瀵煎嚭ARMxDSP娑堟伅闃熷垪淇℃伅銆� *
 * 瀵煎嚭ARMxDSP闃熷垪淇℃伅锛屼互渚緿SP鍙互璁块棶闃熷垪銆� *
 * @return 0 if no error.
 */
int ost_exportDSP_mq(void)
{
    int retVal = 0;

    /* 灏咥RMxDSP闃熷垪淇℃伅澶嶅埗鍒板叡浜┖闂达紝浠ヤ究DSP鍙互璁块棶瀹�*/
    retVal = armxdsp_export_queues();
    return retVal;
}

/**
 * @brief 閿�瘉ARMxDSP闃熷垪淇℃伅銆� *
 * 閿�瘉ARMxDSP闃熷垪骞堕噴鏀鹃槦鍒椾娇鐢ㄧ殑鍐呭瓨銆� *
 * @return 鏃�
 */
void ost_destroyDSP_mq(void)
{
    int i;

    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        mq_free(in_queues[i]);
        mq_free(out_queues[i]);
    }
}

int DataTransfer (uint8_t *data, uint32_t size)
{
    uint32_t multiple = size/4;
    uint8_t remainder = size%4;
    uint8_t b[4] ={0};
    uint32_t i = 0;

if(size <= 0)
    return  -1;

if(remainder >0)
    multiple++;

for(i=0; i < multiple; i++)
{
    b[0] =data[0+ i*4] ;
    b[1] =data[1+ i*4] ;
    b[2] =data[2+ i*4] ;
    b[3] =data[3+ i*4] ;

    data[0+ i*4] =  b[3];
    data[1+ i*4] =  b[2];
    data[2+ i*4] =  b[1];
    data[3+ i*4] =  b[0];
}

 return  0;
}
